package novatel

import (
	"dgo/goutils/gnss"
	"dgo/goutils/utils"
	"encoding/binary"
	"fmt"
)

type NovAtelRangeB struct {
	ObsN uint32
}

/*
*

	bufl : 44
*/
func (this *NovAtelRangeB) DecodeObs(week, tow uint32, buf []byte, transVars *gnss.GnssYObsTransVarCollect, obs *gnss.YObsSignal) error {
	trackval := gnss.U4FromBuf(buf[40:])
	/*
			*track =stat&0x1F;
		    *plock =(stat>>10)&1;
		    *parity=(stat>>11)&1;
		    *clock =(stat>>12)&1;
		    satsys =(stat>>16)&7;
		    *halfc =(stat>>28)&1;
		    sigtype=(stat>>21)&0x1F;
	*/
	sys := trackval >> 16 & 7
	signtype := (trackval >> 21) & 0x1F // 0x1F 0001 1111
	plock := utils.GetBitValue0(uint64(trackval), 10)
	clock := utils.GetBitValue0(uint64(trackval), 12)

	/**
	Parity known flag 0 = Not known1 = Known
	*/
	//parity := utils.GetBitValue0(uint64(trackval), 11);

	/**
	Carrier phase measurement 0 = Half Cycle Not Added,	1 = Half Cycle Added
	*/
	halfc := utils.GetBitValue0(uint64(trackval), 28)

	/*
		func ToNaviSys(v byte) byte {
			switch v {
			case 0:
				return gnss.SYS_GPS
			case 1:
				return gnss.SYS_GLO
			case 2:
				return gnss.SYS_SBS
			case 3:
				return gnss.SYS_GAL
			case 4:
				return gnss.SYS_BD
			case 5:
				return gnss.SYS_QZS
			}
			return gnss.SYS_UNKOWN
		}
	*/
	obs.NaviSys = ToNaviSys(byte(sys))
	prn := gnss.U2FromBuf(buf)
	//if obs.NaviSys == gnss.SYS_GLO{
	//	prn -= 37;
	//}else if obs.NaviSys == gnss.SYS_BD{
	//	prn -=140;
	//}else if obs.NaviSys == gnss.SYS_SBS{
	//	prn -=119;
	//}
	if obs.NaviSys == gnss.SYS_GLO {
		prn -= 37
	} else if obs.NaviSys == gnss.SYS_QZS {
		prn -= 192
	}

	if prn <= 0 || prn > 255 {
		return fmt.Errorf("错误的卫星号[%d]", prn)
	}
	obs.SatId = byte(prn)

	// sat=satno(sys,prn)
	_, obs.SigId = RangecmpToYObsSignID(obs.NaviSys, byte(signtype))

	if obs.SigId == 0 {
		return fmt.Errorf("错误的信号类型:%d:%d", obs.NaviSys, signtype)
	}

	// 如果obs.SigId等于0,则未添加卫星系统
	//if obs.SigId == 0 {
	//	fmt.Println(1)
	//}
	/*
		gfrq =U2(p+ 2);
		psr  =R8(p+ 4);
		adr  =R8(p+16);
		dop  =R4(p+28);
		snr  =R4(p+32);
		lockt=R4(p+36);
	*/

	//gfrq := float64(gnss.U2FromBuf(buf[2:]))
	//fmt.Println(gfrq)
	/*
		if (sys==SYS_GLO&&raw->nav.geph[prn-1].sat!=sat) {
			raw->nav.geph[prn-1].frq=gfrq-7;
		}
	*/
	//if obs.NaviSys == gnss.SYS_GLO && obs.SatId != obs.SigId{
	//	frq := gfrq-7;
	//	fmt.Println(frq)
	//}
	obs.PsV = ByteToFloat64(buf[4:])
	obs.PhV = -ByteToFloat64(buf[16:])

	obs.Doppler = float64(ByteToFloat32(buf[28:]))
	obs.Cnr = ByteToFloat32(buf[32:])
	lockt := float64(ByteToFloat32(buf[36:]))
	obs.LkT = int64(lockt * 1000)

	if plock == 0 {
		obs.PhV = 0
		obs.Doppler = 0
	}

	if clock == 0 {
		obs.PsV = 0
	}
	if transVars != nil {
		preVar := transVars.GetVar(obs.NaviSys, byte(prn), obs.SigId)

		if preVar.Tow != 0 {
			t := int64(gnss.TowDiff(preVar.Tow, tow))
			if obs.LkT-preVar.LockT+50 <= t {
				obs.LLI = 1
			} else {
				obs.LLI = 0
			}
		}

		if preVar.Half != halfc {
			obs.LLI |= 1
		}
		preVar.Half = halfc
		preVar.LockT = obs.LkT
		preVar.Tow = tow
		preVar.Week = week
	}
	return nil
}

func (this *NovAtelRangeB) DecodeBuf(week, tow uint32, buf []byte, transVars *gnss.GnssYObsTransVarCollect, onobs func(idx int, yobs *gnss.YObsSignal, err error) bool) error {
	this.ObsN = binary.LittleEndian.Uint32(buf)
	n := int(this.ObsN)
	l := 4 + n*44
	if len(buf) < l {
		return fmt.Errorf("oem4 rangeB length error: len=%d nobs=%d", len(buf), this.ObsN)
	}

	byteN := 4
	var obs gnss.YObsSignal
	for i := 0; i < n; i++ {
		// 初始化
		obs.Reset()
		err := this.DecodeObs(week, tow, buf[byteN:], transVars, &obs)
		if !onobs(i+1, &obs, err) {
			break
		}
		byteN += 44
	}
	return nil
}
